#ifndef     __ARM_CTRL_H
#define     __ARM_CTRL_H

#include "./error.h"
#include "./alg.h"
#include "./prog.h"
#include "../driver/cmd_uart.h"

namespace utils {

template <typename Arm>
class ArmCtrl {
public:
    static constexpr uint32_t Max = 100; // 最大控制百分比
    static constexpr uint16_t ORIGIN_POSITION = 512;
    static constexpr uint16_t ADJ_RESET_VAL = 0xFFFF;
    static constexpr uint16_t ERR_RANGE = 1;
    static constexpr uint32_t TIMEOUT_DURATION = 20 * 1000L;

    enum class dir_t {
        ToMin = 0,
        ToMax = 1,
        Brake = 2,
    };

    ArmCtrl() {
        m_kp = 1.75f;
    }

    inline error_t reset() {
        return run(ORIGIN_POSITION);
    }

    error_t run(uint16_t position, uint16_t min) {
        m_ctrl_min = min;
        return run(position);
    }

    error_t run(uint16_t position) {
        error_t error = is_position_ok(position);
        if (error) {
            return error;
        }
        init();
        prog_t time_prog;
        
        for (;;) {
            uint16_t cur = get_position();
            uint16_t err = alg::abs(position, cur);
            if (err <= ERR_RANGE) {
                break;
            }
            uint16_t output = pid_exec(err);
            if (position > cur) {
                set_dir(dir_t::ToMax);
            } else if (position < cur) {
                set_dir(dir_t::ToMin);
            } 
            set_voltage(output);
           // driver::cmdUart->throw_log(0, "err:%d out:%d", err, output);
            osDelay(100);

            if (time_prog.is_after(TIMEOUT_DURATION)) {
                error = ec::Timeout;
                break;
            }
            if ((output == Max) && (m_lock_count >= 15)) {
                error = ec::Locked;
                break;
            }
        }
        set_dir(dir_t::Brake);
        set_voltage(0);
        return error;
    }

    uint16_t get_position() {
        int16_t val = static_cast<Arm *>(this)->get_raw_position() 
            + ORIGIN_POSITION 
            - m_adj_val;
        if (val > 1024) {
            return val - 1024;
        } else if (val < 0) {
            return val + 1024;
        }
        return val;
    }

    inline uint8_t get_ctrl_min() {
        return m_ctrl_min;
    }

    uint8_t is_position_ok(uint16_t position) {
        if (m_adj_val == ADJ_RESET_VAL) {
            return ec::RobotArmNotAdj;
        }    
        if (position < Arm::POSITION_MIN) {
            return ec::PositionFail;
        }
        if (position > Arm::POSITION_MAX) {
            return ec::PositionFail;
        }
        return ec::Ok;
    }

private:
    uint16_t pid_exec(uint16_t err) {
        if ( alg::abs(err, m_last_err) <= ERR_RANGE ) {
            m_sum += 1;
            m_lock_count += 1;
        }  else {
      //      m_sum = 0;
            m_lock_count = 0;
        }
        int16_t output = m_ctrl_min
            + err * m_kp 
            + m_sum 
            ;
        m_last_err = err;
        if (output > Max) {
            output = Max;
        }
        if (output < m_ctrl_min) {
            output = m_ctrl_min;
        }
        return output;
    }

    void init() {
        m_last_err = 0;
        m_sum = 0;
        m_lock_count = 0;
    }

    inline void set_dir(dir_t dir) {
        static_cast<Arm *>(this)->set_dir(dir);
    }
    inline void set_voltage(uint8_t val) {
        static_cast<Arm *>(this)->set_voltage(val);
    }

protected:
    uint16_t m_adj_val;
    uint32_t m_ctrl_min;
    
private:
    float m_kp;

    uint16_t m_last_err;
    uint16_t m_sum;
    uint32_t m_lock_count;
};

}

#endif
